#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/Empty.h>
#include <std_msgs/String.h> 
#include <std_msgs/Header.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include "serial_ground/Cmd_send.h"
#include "serial_ground/Uav_state.h"
#include <iostream>
#include <fstream>
#define STATE_UAV 101
#define CMD_UAV 102
#define ARM 1
#define TAKEOFF 2
#define LAND 3
#define POS 4
#define VEL 5
#define ACC 6
#define POS_VEL 7
#define POS_ACC 8
#define VEL_ACC 9
#define POS_VEL_ACC 10
#define ATT 11


#define LENTH_STATE_UAV 127
#define LENTH_CMD_UAV 73

static uint8_t s_buffer[LENTH_CMD_UAV];	//分配静态存储空间
static uint8_t recv_buffer[LENTH_STATE_UAV];	//分配静态存储空间
serial::Serial ser; //声明串口对象 
int hour,minute,second;
std_msgs::Int32 time_delay;

int check_sum(unsigned char *buff, int len)
{
    int sum = 0;
    for (int i=0; i<len; i++)
    {
        sum = (sum + buff[i]) & 0xffffffff;
    }
    return sum & 0xffffffff;
}

//回调函数 
void write_callback(const serial_ground::Cmd_send::ConstPtr& msg)
{ 
      // ROS_INFO_STREAM("Writing to serial port" << *msg); 
      memset(s_buffer,0,sizeof(s_buffer));
      s_buffer[0] = 0xff;
      s_buffer[1] = 0xfe;
      s_buffer[2] = (msg->frame_id & 0xff);     //frame_id
      s_buffer[3] = (msg->uav_index & 0xff);      //uav_index
      s_buffer[4] = (msg->cmd_type & 0xff);      //cmd_type
      memcpy((s_buffer+5), &(msg->exp_local_x), 8);     //exp_local_x
      memcpy((s_buffer+13), &(msg->exp_local_y), 8);    //exp_local_y
      memcpy((s_buffer+21), &(msg->exp_local_z), 8);     //exp_local_z
      memcpy((s_buffer+29), &(msg->exp_local_vx), 8); //exp_local_vx
      memcpy((s_buffer+37), &(msg->exp_local_vy), 8); //exp_local_vy
      memcpy((s_buffer+45), &(msg->exp_local_vz), 8); //exp_local_vz
      memcpy((s_buffer+53), &(msg->exp_yaw), 4);  //exp_yaw

      time_t tt = time(NULL);
      tm* t = localtime(&tt);
      int hour = t->tm_hour;
      int minutes = t->tm_min;
      int seconds = t->tm_sec;
      memcpy((s_buffer+57), &(hour), 4);
      memcpy((s_buffer+61), &(minutes), 4);
      memcpy((s_buffer+65), &(seconds), 4);
      // std::cout << "hour=" << hour << std::endl;
      // std::cout << "minutes=" << minutes << std::endl;
      // std::cout << "seconds=" << seconds << std::endl;

      int sum_end = check_sum(s_buffer, LENTH_CMD_UAV-4);
      memcpy((s_buffer+LENTH_CMD_UAV-4), &(sum_end), 4);

      ser.write(s_buffer, LENTH_CMD_UAV);   //发送串口数据
}

void Decode_frame(serial::Serial& ser, serial_ground::Uav_state& result, std_msgs::Int32& time_delay)
{
      static std::vector<uint8_t> last_remaining_buffer;
      std::vector<uint8_t> cat_buffer;
      size_t bytes_read;

      // if (ser.available() >= LENTH_STATE_UAV) {
      //       bytes_read = ser.read(recv_buffer, LENTH_STATE_UAV);
      // }
      // else {
      //       return;
      // }

      bytes_read = ser.read(recv_buffer, LENTH_STATE_UAV);

      std::cout << "ser.available()=" << ser.available() << std::endl;

      std::cout << "bytes_read=" << bytes_read << std::endl;

      cat_buffer.reserve(last_remaining_buffer.size() + bytes_read); //预留空间

      cat_buffer.insert(cat_buffer.end(), last_remaining_buffer.begin(), last_remaining_buffer.end()); //将last_remaining_buffer中的数据插入到cat_buffer中

      cat_buffer.insert(cat_buffer.end(), recv_buffer, recv_buffer+bytes_read); //继续将recv_buffer中的数据插入到cat_buffer中

      int data_len = cat_buffer.size();

      // 找包头
      int i = 0;
      for(i; i < data_len-2; i++){
            if(cat_buffer[i] ==0xff && cat_buffer[i+1] == 0xfe){

                  if(data_len - i + 1 >= LENTH_STATE_UAV){// 判断是否存在完整帧
            
                  int sum_check, sum_end;
                  // 求和校验
                  sum_check = check_sum(cat_buffer.data(), LENTH_STATE_UAV-4);
                  // 根据包头确定校验位和包尾是否符合协议满足协议要求
                  memcpy(&sum_end, &cat_buffer[LENTH_STATE_UAV-4], 4); //sum_end

                        if(sum_check == sum_end){
                        // 将数据存入result
                        result.frame_id = (uint8_t)cat_buffer[2];
                        result.uav_index = (uint8_t)cat_buffer[3];
                        result.connected = (uint8_t)cat_buffer[4];
                        result.armed = (uint8_t)cat_buffer[5];
                        result.mode = (uint8_t)cat_buffer[6];
                        memcpy(&(result.latitude), &cat_buffer[7], 8);     //latitude
                        memcpy(&(result.longitude), &cat_buffer[15], 8);    //longitude
                        memcpy(&(result.altitude), &cat_buffer[23], 8);     //altitude
                        memcpy(&(result.rel_altitude), &cat_buffer[31], 8);  //rel_altitude
                        memcpy(&(result.deg_compass), &cat_buffer[39], 8); //deg_compass
                        memcpy(&(result.local_x), &cat_buffer[47], 8);     //local_x
                        memcpy(&(result.local_y), &cat_buffer[55], 8);     //local_y
                        memcpy(&(result.local_z), &cat_buffer[63], 8);     //local_z
                        memcpy(&(result.local_lin_x), &cat_buffer[71], 8); //local_lin_x
                        memcpy(&(result.local_lin_y), &cat_buffer[79], 8); //local_lin_y
                        memcpy(&(result.local_lin_z), &cat_buffer[87], 8); //local_lin_z
                        memcpy(&(result.ground_speed), &cat_buffer[95], 8); //ground_speed
                        memcpy(&(result.air_speed), &cat_buffer[103], 8);   //air_speed
                        memcpy(&(result.hour), &cat_buffer[111], 4); //hour
                        memcpy(&(result.minutes), &cat_buffer[115], 4); //minutes
                        memcpy(&(result.seconds), &cat_buffer[119], 4); //seconds

                        ROS_INFO_STREAM("Read: \n" << result); 
                        // memcpy(&sum_end, &cat_buffer[123], 4); //sum_end

                        time_t t_now = time(NULL);
                        tm* t = localtime(&t_now);
                        int hour = t->tm_hour;
                        int minutes = t->tm_min;
                        int seconds = t->tm_sec;

                        // std_msgs::Int32 time_delay; // 计算时延
                        if (result.seconds <= seconds)
                              time_delay.data = abs(seconds - result.seconds);
                        else
                              time_delay.data = 60 - abs(seconds - result.seconds);

                        // 将剩余数据保留，
                        last_remaining_buffer.clear(); // 清空元素，buffer1.size() = 0;
                        last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + (i + LENTH_STATE_UAV), cat_buffer.end()); // 保留剩下的数据
                        return;
                        }
                  }
                  else{
                  // 如果不存在完整帧，则将剩余数据保存
                  last_remaining_buffer.clear();
                  last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + i, cat_buffer.end()); // 保留剩下的数据
                  return;
                  }
            }
      }
      // 合并两个vector，两帧拼接
      // 没找到包头，将最后两个字节数据保留，防止漏掉帧头
      if(i == data_len-2){
      last_remaining_buffer.clear();
      last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + i,  cat_buffer.end());
      }
}

int main (int argc, char** argv)
{
      //初始化节点 
      ros::init(argc, argv, "serial_node"); 
      //声明节点句柄 
      ros::NodeHandle nh; 
      //订阅主题，并配置回调函数 
      ros::Subscriber write_sub = nh.subscribe("/outdoor/uav/cmd", 1, write_callback);
      //发布主题
      ros::Publisher read_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav/state", 1);
      ros::Publisher time_delay_pub = nh.advertise<std_msgs::Int32>("/outdoor/uav/time_delay", 1);
      // ros::Publisher uav1_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav1/state", 1000);
      // ros::Publisher uav2_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav2/state", 1000);
      // ros::Publisher uav3_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav3/state", 1000);
      // ros::Publisher uav4_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav4/state", 1000);
      // ros::Publisher uav5_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav5/state", 1000);
      // ros::Publisher uav6_state_pub = nh.advertise<serial_ground::Uav_state>("/outdoor/uav6/state", 1000);
      // ros::Publisher uav_state_pub[6] = {uav1_state_pub, uav2_state_pub, uav3_state_pub, uav4_state_pub, uav5_state_pub, uav6_state_pub};
      try 
      { 
            //设置串口属性，并打开串口 
            ser.setPort("/dev/ttyUSB0"); 
            ser.setBaudrate(57600); 
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
      } 
      catch (serial::IOException& e) 
      { 
            ROS_ERROR_STREAM("Unable to open Serial port !"); 
            return -1; 
      } 
      //检测串口是否已经打开，并给出提示信息 
      if(ser.isOpen()) 
      { 
            ROS_INFO_STREAM("Serial Port initialized"); 
      } 
      else 
      { 
            return -1; 
      }

      ros::Rate loop_rate(50);
      
      while(ros::ok()) 
      {
            if(ser.available()){
                  // ROS_INFO_STREAM("Reading from serial port\n");
                  serial_ground::Uav_state result;
                  std_msgs::Int32 time_delay;
                  Decode_frame(ser, result, time_delay);
                  // ROS_INFO_STREAM("Read: \n" << result); 

                  if (result.frame_id==STATE_UAV)
                  {
                        // result.header.stamp = ros::Time::now ();
                        read_pub.publish(result);
                        time_delay_pub.publish(time_delay);
                        // uav_state_pub[result.uav_index-1].publish(result);
                  }
            }

            //处理ROS的信息，比如订阅消息,并调用回调函数 
            ros::spinOnce(); 
            loop_rate.sleep(); 
      }
}